//
// Created by jg on 2021/3/2.
//

#ifndef ACTIVE_LOCATION_PATH_ACTIVE_LOCATION_PATH_H
#define ACTIVE_LOCATION_PATH_ACTIVE_LOCATION_PATH_H

#include <iostream>
#include <eigen3/Eigen/Dense>
#include <memory>
#include <vector>
#include <algorithm>
#include <float.h>

using std::vector;
using std::shared_ptr;

namespace alpath_graph{
    class Edge;

/* ---------- node of topo graph ---------- */
    class GraphNode {
    private:
        /* data */

    public:
        enum NODE_STATE{
            FAR = 1, OPEN, CLOSE
        };

        GraphNode(/* args */) {
            parent_ = nullptr;
        }

        GraphNode(Eigen::Vector3d pos, int id) {
            pos_  = std::move(pos);
            id_   = id;

            parent_ = nullptr;
        }

        ~GraphNode() {
        }
        //graph
        vector<shared_ptr<GraphNode>> neighbors_;
        vector<shared_ptr<Edge>>      edges_;
        //node parameter
        Eigen::Vector3d pos_;
        int             id_;
        NODE_STATE      state_;
        double          location_quality_;

        typedef shared_ptr<GraphNode> Ptr;

        GraphNode::Ptr parent_;

        void addNeighbor(GraphNode::Ptr& node){
            if(node->id_ == this->id_){
                std::cout << "[graph node]: add neighbors error!, the id is "
                          << node->id_ << std::endl;
                return;
            }

            this->neighbors_.push_back(node);
        }

    };
/* ---------- edge of topo graph ---------- */
    class Edge{
    protected:
        double weight_, length_, alpha_;
        std::pair<GraphNode::Ptr, GraphNode::Ptr> nodes_;
    public:

        Edge(){}
        Edge(double alpha, GraphNode::Ptr& node1, GraphNode::Ptr& node2){
            nodes_  = std::make_pair(node1, node2);
            alpha_  = alpha;
            length_ = (nodes_.first->pos_ - nodes_.second->pos_).norm();
            weight_ = length_ * alpha_;
        }

        typedef shared_ptr<Edge> Ptr;

        double getWight(){return weight_;}

        int getNodeID1(){return nodes_.first->id_;}

        int getNodeID2(){return nodes_.second->id_;}
    };
}

class AL_Path{
protected:
    Eigen::Vector3d start_, end_;
    int start_id_, end_id_;

    bool is_use_alpath_;

    vector<shared_ptr<alpath_graph::GraphNode>> gragh_;
    vector<Eigen::Vector3d> raw_path_;
public:
    AL_Path():is_use_alpath_(true){}
    ~AL_Path(){}

    void init(bool _is_use_alpath);

    vector<Eigen::Vector3d> findPath(Eigen::Vector3d& start, Eigen::Vector3d& end);

    vector<Eigen::Vector3d> getRawPath() {return raw_path_;}
protected:
    void createEdge(double alpha, alpath_graph::GraphNode::Ptr& node1, alpath_graph::GraphNode::Ptr& node2);

    void resetGraph();

    void searchNeighbors(alpath_graph::GraphNode::Ptr current, vector<alpath_graph::GraphNode::Ptr>& open_vec);

    void trackBackPath(vector<Eigen::Vector3d>& path, vector<int>& path_id);

};

#endif //ACTIVE_LOCATION_PATH_ACTIVE_LOCATION_PATH_H
